#include "sim.hpp"
#include "../utils/utils.hpp"
#include "shuf.hpp"

Oracle::Oracle(const Rescue &w) {
	double starttime = walltime();
	auto p = w.solve();
	ops = p.ops;
	dfpair(stdout, "oracle planning time", "%g", walltime()-starttime);
	std::reverse(ops.begin(), ops.end());
}

boost::optional<Op> Oracle::plan(const Rescue&) {
	if (ops.empty())
		return boost::optional<Op>();
	auto o = ops.back();
	ops.pop_back();
	return boost::optional<Op>(o);
}

boost::optional<Op> DepthFirst::plan(const Rescue &r) {
	auto ops = r.ops();

	shuf(ops);

	// Remove ops for which we don't have time.
	for (unsigned int i = 0; i < ops.size(); i++) {
		if (!havetime(r, ops[i])) {
			ops[i] = ops.back();
			ops.pop_back();
			i--;
		}
	}
	assert (ops.size() > 0);

	if (ops.size() == 1)
		return ops[0];

	// We can do more than 1 thing, so remove any Finish
	// operators.  We don't want to finish until we have to.
	for (unsigned int i = 0; i < ops.size(); i++) {
		if (!ops[i]) {
			ops[i] = ops.back();
			ops.pop_back();
			i--;
		}
	}

	assert (ops.size() > 0);

	// Always report if we can.
	for (auto o = ops.begin(); o != ops.end(); o++) {
		auto op = *o;
		if (op && op->kind == Op::Report)
			return op;
	}

	// Go to unvisited rooms if there are any
	for (auto o = ops.begin(); o != ops.end(); o++) {
		assert (*o);
		assert ((*o)->kind == Op::Move);

		if ((int)r.visited.size() > (*o)->value && !r.visited[(*o)->value])
			return *o;
	}

	// OK, visited everywhere, take the action that leads
	// back closest to the home node.
	Graph g = r.m.rescuegraph();
	g.precompute(r.home);
	boost::optional<Op> best;
	double bestdist = std::numeric_limits<double>::infinity();
	for (auto o = ops.begin(); o != ops.end(); o++) {
		double d = g.shortest[r.home][(*o)->value].dist;
		if (d < bestdist) {
			bestdist = d;
			best = *o;
		}
	}

	return best;
}

bool DepthFirst::havetime(const Rescue &r, boost::optional<Op> op) {
	Graph g = r.m.rescuegraph();
	g.precompute(r.home);

	double t = 0;
	if (op) {
		int dst = r.loc;
		switch (op->kind) {
		case Op::Move: {
			const Map::Edge *e = r.m.edge(r.loc, op->value);
 				assert (e);
			t = e->line.length();
			dst = op->value;
			break;
		}

		case Op::Report:
			t = ReportTime;
			break;

		default:
			fatal("DepthFirst.havetime: invalid op kind: %d", op->kind);
		}
		t += g.shortest[r.home][dst].dist;
	}
	return r.time+t <= r.deadline;
}

Hindsight::Hindsight(unsigned int n, unsigned int s, bool r) :
	reuse(r), N(n), S(s) {
}

boost::optional<Op> Hindsight::plan(const Rescue &w) {
	if (!reuse || futures.size() < N || !last.consistent(w)) {
		fprintf(stderr, "\trolling new futures\n");
		futures.clear();
		for (unsigned int i = 0; i < N; i++) {
			auto f = w.future(S);
			futures.push_back(f);
		}
		last = w;
	} else {
		fprintf(stderr, "\treusing futures\n");
	}

	auto ops = w.ops();
	shuf(ops);

	boost::optional<Op> best;
	CostTime bcost = CostTime::inf();
	for (auto o = ops.begin(); o != ops.end(); o++) {
		if (*o)
 			fprintf(stderr, "\tplanning for %s", (*o)->str().c_str());
		else
			fprintf(stderr, "\tplanning for quitting");

		CostTime e(0, 0);
		for (auto f = futures.begin(); f != futures.end(); f++) {
			Rescue cpy(*f);
			cpy.act(*o);
			auto sol = cpy.solve();
			e += sol.ct;
			fprintf(stderr, ".");
		}
		e /= CostTime(futures.size(), futures.size());

 		fprintf(stderr, "(%g,%g)\n", e.cost, e.time);
		if (e < bcost) {
			best = *o;
			bcost = e;
		}
	}

	std::string str = "<none>";
	if (best)
		str = best->str();
	fprintf(stderr, "\tselecting: %s (%g,%g)\n", str.c_str(), bcost.cost, bcost.time);

	for (auto f = futures.begin(); f != futures.end(); f++)
		f->act(best);

	return best;
}

// Sim simulates a robot moving in the
// given world using the given planner
// to chose its next action.
void sim(Rescue w, Planner *planner, bool dump) {
	Rescue known = w.initallyknown();
	std::vector<Op> ops;

	if (dump)
		w.draw("map.eps", ops);

	dfrowhdr(stdout, "step", 3, "number", "simulation time", "wall time");

	double mintime = std::numeric_limits<double>::infinity();
	double maxtime = -std::numeric_limits<double>::infinity();
	double starttime = walltime();
	for (;;) {
		known.sense(w);

		if (dump) {
			char buf[128];
			snprintf(buf, sizeof buf, "map%05u.eps", (unsigned int) ops.size());
			known.draw(buf, ops);
		}

		double stepstart = walltime();
		auto op = planner->plan(known);
		known.validateop(op, ops);

		if (!op) {
			if (!dump)	// Just save the last one if not dumping.
				w.draw("map.eps", ops);
			break;
		}

		double steptime = walltime() - stepstart;
		mintime = std::min(steptime, mintime);
		maxtime = std::max(steptime, maxtime);

		fprintf(stderr, "%u: %s (time=%g, walltime=%gs)\n",
			(unsigned int) ops.size(), op->str().c_str(), w.time,
			steptime);

		ops.push_back(*op);
		w.act(*op);
		known.act(*op);

		dfrow(stdout, "step", "ugg", (unsigned long) ops.size(),
			w.time, steptime);
	}
	w.done(stdout, ops);
	dfpair(stdout, "final sol length", "%u", (unsigned int) ops.size());
	dfpair(stdout, "total wall time", "%g", walltime()-starttime);
	dfpair(stdout, "min step time", "%g", mintime);
	dfpair(stdout, "max step time", "%g", maxtime);
}

